#ifndef CONTROLADORESPID_H_
#define CONTROLADORESPID_H_

/************************************************************************************
*	Copyright Paxs Electronics 2012													*
*   All Rights Reserved																*
*   The Copyright symbol does not also indicate public availability or publication.	*
* 																					*
* 								PAXS ELECTRONICS									*
* 																					*
* - Driver:   			PID control Loop											*
* 																					*
* - Compiler:           Code Composer Studio (Licensed)								*
* - Supported devices:  All Stellaris Family Cortex-M3.								*
* - AppNote:			AVR221 - Discrete PID Controller							*
*																					*
* 	Created by: 		Juan Francisco Gomez Ruiz									*
*   Date Created: 		1 - MAR - 2012												*
*   Contac				pacoduz@gmail.com											*
* 																					*
* 	Description: 		PID Control Loop for each angle to control.					*							
*   Device supported and tested: - LM3S5749 - LM3S3748 -							*
* 																				2012*
*************************************************************************************

*************************************************************************************
* 	Revision History:
*
*   Date          CP#           Author
*   MM-DD-YY      XXXXX:1       Initials   Description of change
*   -----------   -----------   --------   ------------------------------------
*    03-01-12      	1.0             JFGR	Initial version. 

*************************************************************************************
*
* File/
*	PID Algorithm to controll the QuaXcopter. 4 functions user available: 
* 
* 	-	void VfnPWMModuleConfiguration(void);
* 	**Function to setup the PWM modules, #define DEVICE controlls wich 
* 	which modules to setup.
* 
* 	-	void VfnPIDGainsConfiguration(void);
* 	**Fucntion to Configurate P,I & D gain parameters for each variable
* 	Pitch, Roll, Yaw & altitude.
* 
* 	-	void VfnMainMotorsSpeed(short MotorsSpeed);
* 	**Fucntion to controll all Seep motors. 
* 	***Modify QuadThrust variable.
* 
* 	-	void VfnStabilize(void);
* 	**Main Fucntion, that runs all PID controllers.
* 	
* 	************************	INPUTS & OUTPUTS	**********************
* 	Inputs (Read/Write):
* 	-	SetPitchAngle, SetRollAngle & SetYawAngle as _iq variables 
* 	NOTE: SetAngles  **MUST BE** in radians.
* 	**Indicate user target angle for each axis.
* 
* 	-	QuadThrust as unsigned short variable.
* 	**Variable aceptable from 0 to 100;
* 
* 	Outputs:
* 	None.
* 	**********************************************************************
* 
* 	DESCRIPTION OF PRIVATE VARIABLES & FUNCTIONS (IN .c FILE)
* 	
* 	* LastProcessValuePitch,  LastProcessValueRoll &  LastProcessValueYaw. Variables, 	
* 	are to store previos process value, esencial for the PID algorithm
* 
* 	* _iq PIDAlgorithm(_iq SetPoint,_iq ProcessValue, _iq LastProcessValue);
* 	PID algorithm. 3 parameters, one result.
* 
*/

	#include "lm3s5749.h"
	#define   GLOBAL_Q	16
	#include "IQmath/IQmathLib.h"
	
	#include "Imu9DOFDriver.h"
	#include "Dcm.h"
	#include "driverlib/sysctl.h"
	#include "inc/hw_memmap.h"
	#include "driverlib/gpio.h"
	#include "driverlib/pwm.h"
	
/******************************************************************************
*   						MACRO DEFINITIONS
******************************************************************************/	
	#define ON				1
	#define OFF				0
	#define LM3S3748		1
	#define LM3S5749		2
	#define DEVICE			(LM3S5749)
	#define DEBUG_PID		(ON) 		//1 = ON, 0 = OFF
	#define KP_PITCH		(1.45)
	#define KI_PITCH		(0.6)
	#define KD_PITCH		(0.9)
	#define KP_ROLL			(1.625)//(1.625)
	#define KI_ROLL			(0.3)//(0.55)
	#define KD_ROLL			(.75)//(1.05)(0.85)
	#define KP_YAW			(0.0)
	#define KI_YAW			(0.0)
	#define KD_YAW			(0.0)
	
	#define TS_PID		 	(0.004065)			//Period = 1/246Hz
	
	#define I_TERM_MAX		(500.0)
	#define I_TERM_MIN		(-500.0)
	#define PID_MAX			(2000.0)
	#define PID_MIN			(-2000.0)
	#define PID_GAIN_STB	(240.0)
	#define PID_GAIN_ORT	(50.0)	
	#define DEGREES_TO_RAD	(0.0174533125)
	#define OFFSET			( .5 * DEGREES_TO_RAD)
	#define PITCH_FLAG		(1)
	#define ROLL_FLAG		(2)
	#define YAW_FLAG		(3)
	
	#define PERIOD_TIME    		62500			//For a time base "period" of 20ms
	#define BASE_TIME 			3125			//To generate a 1ms pulse width
	#define PWM_COMPENSATION_X	-500		//PWM Compensation - "still haven't found why!"
	#define PWM_COMPENSATION_Y	-450
	
	extern volatile short PwmCompensationX;
	extern volatile short PwmCompensationY;
/******************************************************************************
*   Type Definitions
******************************************************************************/	
	typedef struct
	{
   		_iq KpPitch;
    	_iq KiPitch;
    	_iq KdPitch;
 		_iq KpRoll;
    	_iq KiRoll;
    	_iq KdRoll;
    	_iq KpYaw;
    	_iq KiYaw;
    	_iq KdYaw;
	}sPID;

/******************************************************************************
*   						EXTERNAL VARIABLES
******************************************************************************/
	extern sPID PIDgains;
	extern unsigned short QuadThrust;								//QuadRotor Main speed W/R
	extern volatile _iq    SetPitchAngle,SetRollAngle,SetYawAngle;	//QuadRotor desire angles W/R - ** MUST BE RADIANS **

/******************************************************************************
*   					GLOBAL FUNCTION PROTOTYPES
******************************************************************************/	
	void VfnPWMModuleConfiguration(void);				//Function for PWM modules configuration
	void VfnPIDGainsConfiguration(void);				//Function to initiate PID gains
	void VfnMainMotorsSpeed(short MotorsSpeed);			//Function to setup the main speed in all motors
	
	void VfnSetQuadThrust(unsigned short Thrust);		//Set the QuadRotor Thrust inderectly "Not modifing PWM modules"
	unsigned char VfnGetQuadThrust(void);				//Function that return the QuaXcopter Thrust percent.
	void VfnGetMotorsThrust(unsigned short* MotorThrustp);//Function that write into pointer the thrust percent of each motor.
	
	void VfnStabilize(void);
	
	#if DEBUG_PID == 1
		extern _iq ErrorPitch,ErrorRoll,ErrorYaw;
		extern _iq ITermPitch,ITermRoll,ITermYaw;
		extern _iq PidP,PidR,PidY;
	#endif

#endif /*CONTROLADORESPID_H_*/
